#include "../src/qfunc.hh"
#include "../src/distribution.cc"
#include "../src/nominal.hh"

#include <cassert>
#include <iostream>

namespace
{
using namespace playerpiano;

struct QFuncTestDatum 
{ 
  struct
    {
      float    alpha[4 * 5 * 5];
      float    logbeta;
      float    pz[5];
      Rating   ratings[8];
      float    logbetamean;
      float    logbetastddev;
    }                   inputs;
  struct
    {
      float q;
      float dqdlogbeta;
      float ddqddlogbeta;
    }                   desired_output;
};


static void
test_qfunc (void)
{
  QFuncTestDatum data[] = 
    {{{{-1., 0.947173, 0.914937, 0.202602, 0.339588, 0.993513, -1.,  0.524504, 0.882454, 0.152066, 0.594485, 0.846559, -1., 0.911473,  0.528953, 0.986873, 0.573407, 0.0255994, -1., 0.850794, 0.876752,  0.695093, 0.0447534, 0.767527, -1., -1., 0.746031, 0.216144,  0.839426, 0.595252, 0.798857, -1., 0.301207, 0.636824, 0.255664,  0.805345, 0.776703, -1., 0.75437, 0.103598, 0.21086, 0.930144,  0.842897, -1., 0.574646, 0.223987, 0.356737, 0.817298,  0.723852, -1., -1., 0.347235, 0.661644, 0.772544, 0.956325,  0.601204, -1., 0.4455, 0.933118, 0.361073, 0.802347,  0.144292, -1., 0.296294, 0.105409, 0.997002, 0.367589,  0.541924, -1., 0.0018106, 0.786142, 0.437445, 0.699027,  0.427165, -1., -1., 0.562155, 0.080708, 0.881729, 0.703314,  0.21492, -1., 0.419064, 0.109185, 0.746989, 0.613716,  0.973565, -1., 0.176067, 0.385916, 0.811369, 0.829272,  0.879774, -1., 0.280507, 0.814366, 0.461683, 0.33785,  0.278696, -1.},  0.0282242, {0.0242382, 0.638823, 0.851531, 0.466069,  0.94353}, {{3, 0}, {1, 1}, {1, 1}, {1, 0}, {0, 2}, {1, 1}, {2,  2}, {1, 2}}, 0.757094, 0.148218}, {-54.7742,  19.0428, -74.0116}}, {{{-1., 0.251149, 0.524466, 0.647909,  0.401229, 0.637434, -1., 0.550901, 0.471842, 0.0153133, 0.826065,  0.721629, -1., 0.592069, 0.734806, 0.0116984, 0.259945,  0.254219, -1., 0.45611, 0.983474, 0.235707, 0.615395,  0.604579, -1., -1., 0.517405, 0.292177, 0.858301, 0.456361,  0.266256, -1., 0.767711, 0.210392, 0.0551317, 0.628822,  0.21681, -1., 0.73855, 0.0398184, 0.802757, 0.495181,  0.146481, -1., 0.305012, 0.791059, 0.235236, 0.892263,  0.848902, -1., -1., 0.807585, 0.999529, 0.276867, 0.244324,  0.29018, -1., 0.707352, 0.418566, 0.787963, 0.0239238,  0.93964, -1., 0.208174, 0.732831, 0.395102, 0.722831,  0.469624, -1., 0.693013, 0.592344, 0.227649, 0.323143,  0.388001, -1., -1., 0.801286, 0.992414, 0.43088, 0.539098,  0.993701, -1., 0.992885, 0.154013, 0.294775, 0.703522,  0.285533, -1., 0.735447, 0.506812, 0.679598, 0.345893,  0.527273, -1., 0.77398, 0.284496, 0.623062, 0.0576495,  0.0809678, -1.},  0.692152, {0.395413, 0.734507, 0.692967, 0.890866,  0.402999}, {{1, 2}, {3, 2}, {2, 3}, {0, 1}, {3, 4}, {0, 1}, {3,  1}, {3, 1}}, 0.303627,  0.153869}, {-62.4533, -60.4931, -115.473}}, {{{-1., 0.897165,  0.410115, 0.149613, 0.859094, 0.193643, -1., 0.124581, 0.414166,  0.352283, 0.514046, 0.778688, -1., 0.886893, 0.578302, 0.229549,  0.155626, 0.829243, -1., 0.497334, 0.537398, 0.760213, 0.0947365,  0.804367, -1., -1., 0.646532, 0.357213, 0.79111, 0.650498,  0.749367, -1., 0.947099, 0.641497, 0.791404, 0.555724,  0.822518, -1., 0.22733, 0.439121, 0.0416782, 0.0438295,  0.340438, -1., 0.860819, 0.812129, 0.888204, 0.511194,  0.363485, -1., -1., 0.274731, 0.127991, 0.416458, 0.559117,  0.628199, -1., 0.770778, 0.625348, 0.908619, 0.878832,  0.823679, -1., 0.983851, 0.117215, 0.323108, 0.00116149,  0.756521, -1., 0.678094, 0.28143, 0.957332, 0.416083,  0.817275, -1., -1., 0.469301, 0.0691283, 0.904889, 0.45379,  0.19457, -1., 0.941137, 0.488431, 0.894673, 0.566371,  0.17036, -1., 0.863083, 0.986054, 0.687539, 0.34668,  0.879231, -1., 0.868839, 0.364431, 0.345519, 0.122711,  0.190745, -1.},  0.083001, {0.388187, 0.706627, 0.373471, 0.6137,  0.319059}, {{0, 4}, {3, 1}, {0, 1}, {1, 3}, {1, 2}, {3, 3}, {1,  1}, {3, 0}}, 0.801739,  0.919681}, {-34.8899, -10.1483, -25.0271}}, {{{-1., 0.419129,  0.377921, 0.313308, 0.0250082, 0.852758, -1., 0.207562, 0.450225,  0.0389543, 0.165219, 0.860881, -1., 0.570994, 0.170115, 0.800788,  0.515362, 0.448283, -1., 0.97937, 0.717787, 0.127175, 0.741656,  0.605899, -1., -1., 0.104087, 0.808117, 0.939917, 0.686218,  0.684958, -1., 0.430196, 0.626609, 0.66121, 0.832199,  0.222634, -1., 0.176383, 0.622256, 0.66698, 0.361753,  0.605389, -1., 0.45214, 0.866193, 0.84639, 0.157106,  0.472771, -1., -1., 0.148406, 0.719215, 0.41545, 0.866872,  0.0443186, -1., 0.911098, 0.475533, 0.180654, 0.359361,  0.480903, -1., 0.848924, 0.519444, 0.527162, 0.258269,  0.672541, -1., 0.897188, 0.860181, 0.896516, 0.0671511,  0.445048, -1., -1., 0.993989, 0.0501256, 0.910045, 0.972277,  0.845583, -1., 0.330911, 0.494595, 0.105406, 0.801264,  0.419812, -1., 0.0190622, 0.924752, 0.441903, 0.93891,  0.170138, -1., 0.405308, 0.914742, 0.680641, 0.497598,  0.50812, -1.},  0.0545605, {0.784125, 0.430446, 0.0630725, 0.0605719,  0.734}, {{1, 0}, {3, 1}, {0, 3}, {0, 0}, {3, 2}, {0, 1}, {1,  1}, {3, 3}}, 0.520401, 0.0907953}, {-42.5806,  45.8803, -142.353}}, {{{-1., 0.214989, 0.403089, 0.0258062,  0.98539, 0.413725, -1., 0.983276, 0.00674406, 0.0606375, 0.971821,  0.0443666, -1., 0.836606, 0.655329, 0.0570799, 0.363726,  0.339008, -1., 0.147209, 0.00251939, 0.5796, 0.908562,  0.0841362, -1., -1., 0.941948, 0.845601, 0.388161, 0.993341,  0.726959, -1., 0.442512, 0.362354, 0.00795138, 0.313234,  0.459235, -1., 0.35561, 0.947314, 0.341412, 0.414869,  0.519004, -1., 0.291985, 0.284332, 0.0511433, 0.179996,  0.144776, -1., -1., 0.281813, 0.471543, 0.271434, 0.06064,  0.339866, -1., 0.625942, 0.883273, 0.067299, 0.612907,  0.18343, -1., 0.520919, 0.0593476, 0.299673, 0.724195,  0.165309, -1., 0.112034, 0.958261, 0.309326, 0.646304,  0.820049, -1., -1., 0.673928, 0.258183, 0.466308, 0.675273,  0.392115, -1., 0.78664, 0.194874, 0.614633, 0.0522499,  0.160697, -1., 0.311601, 0.547334, 0.439343, 0.977267,  0.790682, -1., 0.487986, 0.13967, 0.253073, 0.625373,  0.375952, -1.},  0.181409, {0.943747, 0.979069, 0.555904, 0.50748,  0.685564}, {{1, 1}, {0, 2}, {2, 4}, {1, 3}, {1, 3}, {0, 0}, {3,  0}, {1, 2}}, 0.51276,  0.880631}, {-55.5817, -18.602, -40.6779}}, {{{-1., 0.115365,  0.898925, 0.317886, 0.265998, 0.0631148, -1., 0.738227,  0.00628565, 0.718665, 0.623772, 0.76096, -1., 0.215604, 0.230679,  0.484102, 0.507887, 0.590231, -1., 0.854726, 0.302694, 0.564141,  0.611163, 0.298823, -1., -1., 0.795214, 0.878576, 0.0984021,  0.418192, 0.679849, -1., 0.979652, 0.780516, 0.152194, 0.616734,  0.241424, -1., 0.77423, 0.433529, 0.992962, 0.480464,  0.558626, -1., 0.20285, 0.50886, 0.972577, 0.968395,  0.348124, -1., -1., 0.206166, 0.408436, 0.357232, 0.0493015,  0.410952, -1., 0.52986, 0.25883, 0.63111, 0.731103, 0.550208, -1.,  0.478315, 0.478916, 0.114369, 0.308784, 0.704084, -1., 0.0453871,  0.121407, 0.828319, 0.145458, 0.842537, -1., -1., 0.612547,  0.855742, 0.177063, 0.494412, 0.406381, -1., 0.447306, 0.819831,  0.445111, 0.995429, 0.917446, -1., 0.561001, 0.814001, 0.264326,  0.367238, 0.0826861, -1., 0.335085, 0.149957, 0.0584539, 0.378602,  0.289698, -1.},  0.0285501, {0.230134, 0.233143, 0.447162, 0.416003,  0.374392}, {{1, 3}, {1, 3}, {3, 4}, {3, 3}, {3, 4}, {0, 2}, {2,  2}, {3, 3}}, 0.0560801,  0.952749}, {-23.8974, -6.74177, -15.731}}, {{{-1., 0.00962231,  0.927086, 0.236249, 0.507638, 0.0141936, -1., 0.00963976,  0.675249, 0.693637, 0.749868, 0.642402, -1., 0.592563, 0.358551,  0.599911, 0.583948, 0.213961, -1., 0.0688532, 0.571361, 0.353814,  0.980817, 0.621692, -1., -1., 0.155358, 0.979422, 0.924737,  0.668943, 0.145736, -1., 0.0523357, 0.688488, 0.161304, 0.131542,  0.042696, -1., 0.0132394, 0.467668, 0.381674, 0.400294,  0.420677, -1., 0.109116, 0.781763, 0.816346, 0.206716,  0.0402632, -1., -1., 0.210402, 0.462532, 0.225899, 0.418572,  0.0550435, -1., 0.483111, 0.301162, 0.749629, 0.909308,  0.430775, -1., 0.612674, 0.588325, 0.777766, 0.388079,  0.599434, -1., 0.120657, 0.396092, 0.987785, 0.178757,  0.0115404, -1., -1., 0.614329, 0.171439, 0.972041, 0.971277,  0.403927, -1., 0.708907, 0.746142, 0.552706, 0.348884,  0.225796, -1., 0.444981, 0.803077, 0.439576, 0.795021,  0.832307, -1., 0.214752, 0.66181, 0.406942, 0.232873,  0.0940952, -1.},  0.265719, {0.419156, 0.0541157, 0.0825549, 0.65139,  0.247717}, {{0, 3}, {2, 0}, {3, 1}, {1, 4}, {1, 4}, {0, 3}, {2,  2}, {0, 4}}, 0.0820746,  0.111278}, {-21.5485, -23.8451, -98.9922}}, {{{-1., 0.247462,  0.538811, 0.335932, 0.558572, 0.898579, -1., 0.313015, 0.890951,  0.755496, 0.459003, 0.517994, -1., 0.0586443, 0.540744, 0.797192,  0.111053, 0.825771, -1., 0.446648, 0.531474, 0.691896, 0.771656,  0.364094, -1., -1., 0.880084, 0.444179, 0.689581, 0.252816,  0.632622, -1., 0.905368, 0.353649, 0.694244, 0.734043,  0.592353, -1., 0.462697, 0.938748, 0.27504, 0.0743589,  0.404053, -1., 0.398005, 0.477848, 0.963306, 0.578282,  0.951356, -1., -1., 0.946374, 0.27141, 0.806626, 0.587263,  0.0662901, -1., 0.827231, 0.117045, 0.334447, 0.433668,  0.921863, -1., 0.763396, 0.640203, 0.699625, 0.32951,  0.300699, -1., 0.701455, 0.424585, 0.255151, 0.896646,  0.30345, -1., -1., 0.946737, 0.291845, 0.318364, 0.352094,  0.000362512, -1., 0.020435, 0.511738, 0.764832, 0.934072,  0.193204, -1., 0.394693, 0.430385, 0.500404, 0.271341,  0.631297, -1., 0.790182, 0.800779, 0.941831, 0.330598,  0.0887266, -1.},  0.376194, {0.68668, 0.433953, 0.785276, 0.429457,  0.394835}, {{3, 4}, {0, 2}, {3, 4}, {3, 2}, {1, 1}, {3, 3}, {2,  2}, {3, 1}}, 0.115589,  0.433182}, {-45.5675, -25.1726, -50.1355}}, {{{-1., 0.429095,  0.3744, 0.603851, 0.66835, 0.495022, -1., 0.181196, 0.209158,  0.237965, 0.994618, 0.909855, -1., 0.577861, 0.447784, 0.19384,  0.968024, 0.247262, -1., 0.359057, 0.817646, 0.281345, 0.813309,  0.573781, -1., -1., 0.388188, 0.88651, 0.69772, 0.140599,  0.959094, -1., 0.51211, 0.0938696, 0.472249, 0.464071,  0.330914, -1., 0.884712, 0.234284, 0.469453, 0.421059,  0.306851, -1., 0.7865, 0.275614, 0.453034, 0.0595894,  0.427443, -1., -1., 0.457968, 0.17169, 0.24628, 0.853662,  0.0697796, -1., 0.28518, 0.54856, 0.713063, 0.110686,  0.77307, -1., 0.45469, 0.240814, 0.646614, 0.442156,  0.569978, -1., 0.00653037, 0.177161, 0.0210979, 0.263127,  0.220031, -1., -1., 0.901547, 0.568064, 0.203537, 0.792588,  0.443579, -1., 0.396374, 0.957257, 0.938926, 0.3738,  0.111194, -1., 0.408697, 0.225863, 0.263114, 0.338124,  0.954007, -1., 0.985049, 0.6165, 0.895967, 0.384029,  0.978519, -1.},  0.439338, {0.874869, 0.120902, 0.758488, 0.537791,  0.306806}, {{1, 1}, {2, 3}, {1, 2}, {0, 1}, {2, 3}, {3, 0}, {3,  1}, {1, 1}}, 0.917365,  0.9659}, {-46.9374, -25.7253, -48.3696}}, {{{-1., 0.0942116,  0.910432, 0.960108, 0.0269747, 0.720412, -1., 0.799238, 0.55141,  0.801112, 0.457298, 0.461114, -1., 0.597403, 0.816063, 0.840798,  0.565147, 0.213374, -1., 0.837544, 0.40146, 0.690278, 0.0924724,  0.0790556, -1., -1., 0.863669, 0.383472, 0.175108, 0.113155,  0.769457, -1., 0.473041, 0.215, 0.0861805, 0.0490454,  0.673803, -1., 0.663589, 0.285069, 0.591748, 0.212689,  0.0661859, -1., 0.469006, 0.750949, 0.647542, 0.852811,  0.631462, -1., -1., 0.349489, 0.957264, 0.760339, 0.552407,  0.48582, -1., 0.573791, 0.585232, 0.439252, 0.716363,  0.10075, -1., 0.370232, 0.353071, 0.667318, 0.426947,  0.706642, -1., 0.0680023, 0.0755702, 0.214259, 0.640457,  0.598996, -1., -1., 0.324621, 0.566717, 0.787645, 0.967534,  0.975132, -1., 0.609453, 0.027306, 0.415127, 0.489311,  0.035662, -1., 0.442074, 0.975875, 0.772948, 0.934912,  0.0718427, -1., 0.622804, 0.10563, 0.507964, 0.3652,  0.554802, -1.},  0.0300602, {0.293706, 0.724744, 0.955806, 0.705439,  0.726989}, {{1, 4}, {0, 2}, {1, 1}, {1, 2}, {2, 1}, {1, 3}, {0,  3}, {0, 3}}, 0.937099,  0.988272}, {-48.4881, -11.2107, -28.8461}}}; 

  for (unsigned int i = 0; i < sizeof (data) / sizeof (data[0]); ++i)
    {
      NormalDistribution prior_logbeta (data[i].inputs.logbetamean, 
                                        data[i].inputs.logbetastddev);
      Nominal nominal (data[i].inputs.alpha, 0, 0, 4, 5);

      for (unsigned int j = 0; j < 4; ++j)
        {
          for (unsigned int k = 0; k < 5; ++k)
            {
              data[i].inputs.alpha[j * 5 * 5 + k * 5 + k] = drand48 ();
            }
        }

      float hq = 0;

      for (unsigned int k = 0; k < 5; ++k)
        {
          hq -= data[i].inputs.pz[k] * log (data[i].inputs.pz[k]);
        }

      std::vector<float> pz (data[i].inputs.pz, data[i].inputs.pz + 5);
      std::vector<Rating> ratings (data[i].inputs.ratings,
                                   data[i].inputs.ratings + 8);

      float dqdlogbeta;
      float ddqddlogbeta;

      float q = qfunc (nominal,
                       data[i].inputs.logbeta,
                       dqdlogbeta,
                       ddqddlogbeta,
                       pz,
                       ratings,
                       prior_logbeta,
                       hq);

      assert (fabs (q - data[i].desired_output.q) <=
              1e-4 * (1 + fabs (q) + fabs (data[i].desired_output.q)) ||
              (std::cerr << q << " ?= " << data[i].desired_output.q << std::endl,
               0));

      assert (fabs (dqdlogbeta - data[i].desired_output.dqdlogbeta) <=
              1e-4 * (1 + fabs (dqdlogbeta) + fabs (data[i].desired_output.dqdlogbeta)) ||
              (std::cerr << dqdlogbeta << " ?= " << data[i].desired_output.dqdlogbeta << std::endl,
               0));

      assert (fabs (ddqddlogbeta - data[i].desired_output.ddqddlogbeta) <=
              1e-4 * (1 + fabs (ddqddlogbeta) + fabs (data[i].desired_output.ddqddlogbeta)) ||
              (std::cerr << ddqddlogbeta << " ?= " << data[i].desired_output.ddqddlogbeta << std::endl,
               0));
    }
}

}

int 
main (void) 
{
  srand48 (69);

  test_qfunc ();

  return 0;
}
